#include <ros/ros.h>
#include <iostream>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <gnc_functions2.hpp>
#include <ros/time.h> 
#include <ros/duration.h>
#include <std_msgs/Bool.h>
#include <cmath>
#include <ar_track_alvar_msgs/AlvarMarkers.h>
#include <ar_track_alvar_msgs/AlvarMarker.h>
#include <fstream>//ReadCSVdata
#include <sstream>
#include <iomanip>
#define PI 3.14159265358979323846
using namespace Eigen;
using namespace std;

Eigen::Isometry3d Twa,Tca,Tbc,Twb,Twc;//刚体变换矩阵
Eigen::Matrix3d rotation_matrix_bc,rotation_matrix_wb,rotation_matrix_ca;;
Eigen::Vector3d t_wb,t_ca;
Quaterniond q_wb,q_ca;

ros::Time last_request;//
ros::Time land_request;

std::vector<std::pair<double, double>> waypoints;
int current_waypoint_index = 0;

bool countonlytime=true;//防止land_request一直更新
int count3=0;
bool count1time=true;//防止count3一次多加
double distancetp=0;
double Azimuth=0;
float deg2rad = (M_PI/180);
float temp=0;
bool detect_once=false;
ros::Time hover_start_time;
bool hover_for_calib = false;
float ground_alt=0;
const double HOVER_RADIUS = 0.00003;   // 经纬度偏移量
const int HOVER_POINTS = 4;           // 盘旋航点数
vector<pair<double, double>> hover_waypoints;  // 盘旋航点容器
bool hover_waypoints_generated = false;  

#include <geometry_msgs/Point.h>
geometry_msgs::Point target_point;
std::vector<gnc_api_waypoint> waypoints2;
bool target_received = false;
void targetCallback(const geometry_msgs::Point::ConstPtr& msg)
{
    target_point = *msg;
    target_received = true;
    ROS_INFO("Received target: X=%.2f, Y=%.2f, Z=%.2f", target_point.x, target_point.y, target_point.z);
}

//ReadCSVdata
void load_waypoints_from_csv(const std::string& filename) {
    std::ifstream file(filename);
    if (!file.is_open()) {
        ROS_ERROR("Failed to open waypoint file: %s", filename.c_str());
        return;
    }

    std::string line;
    std::getline(file, line); // 跳过 header

    while (std::getline(file, line)) {
        std::stringstream ss(line);
        std::string lat_str, lon_str;

        if (std::getline(ss, lat_str, ',') && std::getline(ss, lon_str)) {
            double lat = std::stod(lat_str);
            double lon = std::stod(lon_str);
            waypoints.emplace_back(lat, lon);
        }
    }
    ROS_INFO("Loaded %lu waypoints from CSV", waypoints.size());
}


// 实时保存单个 waypoint 到 CSV
void append_waypoint2_to_csv(const std::string& filename, const gnc_api_waypoint& wp) {
    std::ofstream file(filename, std::ios::app);
    if (!file.is_open()) {
        ROS_ERROR("Failed to open file for appending waypoint2.");
        return;
    }
    file << wp.x << "," << wp.y << "," << wp.z << "," << wp.psi << "\n";
    file.close();
}



//将获取的点存为csv文件
void save_waypoints2_to_csv(const std::string& filename, const std::vector<gnc_api_waypoint>& waypoints2) {
    std::ofstream file(filename);
    if (!file.is_open()) {
        ROS_ERROR("Failed to open file for saving waypoints2.");
        return;
    }

    file << "x,y,z,psi\n";
    for (const auto& wp : waypoints2) {
        file << wp.x << "," << wp.y << "," << wp.z << "," << wp.psi << "\n";
    }
    file.close();
    ROS_INFO("Saved %lu waypoints2 to CSV: %s", waypoints2.size(), filename.c_str());
}

// 初始化 CSV（覆盖旧文件，只写一次表头）
void init_waypoints2_csv(const std::string& filename) {
    std::ofstream file(filename, std::ios::out);
    if (!file.is_open()) {
        ROS_ERROR("Failed to open file to init waypoints2 csv.");
        return;
    }
    file << "x,y,z,psi\n";
    file.close();
}
double toRadians(double degree) {
    return degree * PI / 180.0;
}

// 将弧度转换为度数
double toDegrees(double radian) {
    return radian * 180.0 / PI;
}

// 计算方位角（方向角）
double calculateAzimuth(double lat1, double lon1, double lat2, double lon2) {
    // 将经纬度从度数转换为弧度
    lat1 = toRadians(lat1);
    lon1 = toRadians(lon1);
    lat2 = toRadians(lat2);
    lon2 = toRadians(lon2);

    double deltaLon = lon2 - lon1;

    // 使用公式计算方向角
    double x = sin(deltaLon) * cos(lat2);
    double y = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(deltaLon);

    double azimuth = atan2(x, y);  // 结果为弧度

    // 将方位角从弧度转换为度数
    double azimuthDeg = toDegrees(azimuth);

    // 调整结果到 0-360 度范围内
    if (azimuthDeg < 0) {
        azimuthDeg += 360.0;
    }

    return azimuthDeg;
}

void pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    t_wb << msg->pose.position.x ,msg->pose.position.y ,msg->pose.position.z;
    //在Eigen库中,四元数的存储顺序是w、x、y、z
    q_wb = Eigen::Quaterniond(msg->pose.orientation.w ,msg->pose.orientation.x ,msg->pose.orientation.y ,msg->pose.orientation.z);
    t_wb[2]=t_wb[2]-ground_alt;
    rotation_matrix_wb = q_wb.toRotationMatrix();
    Twb.linear() = rotation_matrix_wb;
    Twb.translation() = t_wb;
    ROS_INFO("real_alt=%f",t_wb[2]);
}
void aruco_pos_cb(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr &msg)
{
    
    if(!msg->markers.empty()) {
		ROS_INFO("detected");
        
		detect_state=true;
        const auto& first_marker = msg->markers[0];
		
        t_ca << (first_marker.pose.pose.position.y+0.5*t_wb[2]), 
               (first_marker.pose.pose.position.x), 
               0;
               //first_marker.pose.pose.position.z;
        q_ca = Eigen::Quaterniond(
            first_marker.pose.pose.orientation.w,
            first_marker.pose.pose.orientation.x,
            first_marker.pose.pose.orientation.y,
            first_marker.pose.pose.orientation.z
        );
        ROS_INFO("x_error=,%f,y_error=,%f,z_error=,%f",t_ca[0],t_ca[1],t_ca[2]);
    }
	else{
		detect_state=false;
	}
    rotation_matrix_ca = q_ca.toRotationMatrix();
    Tca.linear() = rotation_matrix_ca;  // 获得旋转矩阵
    Tca.translation() = t_ca;           // 平移矩阵 Tca 为变换矩阵
	//ROS_INFO("x,%f,y,%f,z,%f",t_ca[0],t_ca[1],t_ca[2]);
	
}






// void detceter_cb(const std_msgs::Bool Reached)
// {
//   detect_state=Reached.data;
// }

// #include <GeographicLib/Geoid.hpp>
// double ellipsoidToOrthometric(double lat, double lon, double h) {
//     // EGM96 大地水准面模型（可以换成 "egm2008-1" 等）
//     static const GeographicLib::Geoid geoid("egm96-5", "/usr/local/share/GeographicLib/geoids");
//     double N = geoid.Height(lat, lon);  // 获取指定经纬度的大地水准面起伏
//     double H = h - N;                   // 海拔 = 椭球高 - 起伏
//     return H;
// }

int main(int argc, char** argv)
{
    //load_waypoints_from_csv("/home/jetson/yahboomcar_ws/src/iq_gnc/scripts/waypoints.csv");
    std::string save_file = "/home/jetson/yahboomcar_ws/src/iq_gnc/src/waypoints2.csv";
    //ROS_INFO("wp0=%f",waypoints[0].first);
    // 3) **先初始化 CSV 表头（覆盖旧文件）**
    init_waypoints2_csv(save_file);
	ros::init(argc, argv, "gnc_node");
	ros::NodeHandle gnc_node("~");
    
	init_publisher_subscriber(gnc_node);
  	// wait for FCU connection
	wait4connect();
	//wait for used to switch to mode GUIDED
	wait4start();
	//create local reference frame 
	initialize_local_frame();
	Initialize();
	APMLanding APMLand;
    ros::Subscriber target_sub = gnc_node.subscribe("/target_xyz", 10, targetCallback);
	ros::Subscriber aruco_pose_sub = gnc_node.subscribe<ar_track_alvar_msgs::AlvarMarkers>("/ar_pose_marker", 10, aruco_pos_cb);
	ros::Subscriber position_sub = gnc_node.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 10, pos_cb);  
	mavros_setpoint_pos_pub_ = gnc_node.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 10);
    sleep(1);
    float start_alt=getalt();
    ROS_INFO("GPS起始椭球高度:start_alt:getalt=%f",getalt());
    // double lat = 30.2741;   // 杭州纬度
    // double lon = 120.1551;  // 杭州经度
    //double H = ellipsoidToOrthometric(lat, lon, start_alt);
    //ROS_INFO("h=%f",H);
    ROS_INFO("无人机起始海拔高度:start_alt:getalt=%f",getalt()-26.0);
	takeoff(3);
	ROS_INFO("current_lat=%f, current_lon=%f", get_current_lat(), get_current_lon());
	//ROS_INFO("START");
    ground_alt=t_wb[2];
    ROS_INFO("起飞时地面高度twb[2]:ground_alt=%f",ground_alt);
    ros::Time last_control_time = ros::Time::now();
	sleep(5);
    ROS_INFO("起飞5秒后实际高度real_alt=%f",t_wb[2]);
    // 加载 CSV 文件中的航点
    
    // 如果你没有航点数据，退出程序
    if (waypoints.empty()) {
        ROS_ERROR("No waypoints loaded. Using default wp");
        waypoints = {
        //{30.51905833,120.72055278},
        {30.5179766, 120.7189757},
        {30.5179789,120.7189013},
        {30.5179194,120.7189731},
        {30.5179177,120.7189020}
        };
    }
    
    set_destination_lla(waypoints[0].first, 
                            waypoints[0].second, 
                            start_alt-4, 180);
    
    t_ca << 99, 99, 99;
    int state_flag=0;
	ros::Rate rate(10);
	int counter = 0;
   	
	while(ros::ok())
	{
		//ROS_INFO("state_flag=%d",state_flag);
		ros::spinOnce();
		rate.sleep();
		if(state_flag == 0)
		{
            if (target_received)
            {
                //相机安装x轴超前，z轴朝下
                Eigen::Matrix3d rotation_matrix_bc;//cam->body
                //Eigen::Vector3d
                rotation_matrix_bc << 
                    0,  1,  0,   //xb​=−yc​
                    1, 0,  0,   // yb​=xc​
                    0,  0, -1;   // zb​=zc​

                //相机相对无人机的位置
                Eigen::Vector3d p_cam(target_point.x, target_point.y, target_point.z);
                // 相机 → 机体（仍需旋转）
                Eigen::Vector3d p_body = rotation_matrix_bc * p_cam;  // t_bc = 0
                // 机体 → 世界
                Eigen::Vector3d p_world = rotation_matrix_wb * p_body + t_wb;
                gnc_api_waypoint target_wp;
                target_wp.x = p_world[0];
                target_wp.y = p_world[1];
                target_wp.z = p_world[2]+2.0;//到目标点上方两米
                target_wp.psi = 0;
                ROS_INFO("Saved waypoint2: (%.2f, %.2f, %.2f)", target_wp.x, target_wp.y, target_wp.z);
                if (waypoints2.empty() || (fabs(waypoints2.back().x - target_wp.x) > 0.2 ||fabs(waypoints2.back().y - target_wp.y) > 0.2))
                {
                    waypoints2.push_back(target_wp);//储存第二次飞行的目标点
                    // 实时保存到 CSV
                    append_waypoint2_to_csv(save_file, target_wp);
                    ROS_INFO("Waypoint2 appended to CSV: (%.2f, %.2f, %.2f)", target_wp.x, target_wp.y, target_wp.z);
                }
                    target_received = false;
            }

                
				if (current_waypoint_index < waypoints.size()) {
					distancetp = calculateDistance(waypoints[current_waypoint_index].first,waypoints[current_waypoint_index].second,get_current_lat(),get_current_lon());
					Azimuth=calculateAzimuth(get_current_lat(),get_current_lon(),waypoints[current_waypoint_index].first,waypoints[current_waypoint_index].second);
					ROS_INFO("distancetp=%f",distancetp);
					ROS_INFO("current_waypoint_index=%d",current_waypoint_index);
					if (distancetp < 1.0) {

					current_waypoint_index++;
					}
					else{
						set_destination_lla(waypoints[current_waypoint_index].first, 
	                    waypoints[current_waypoint_index].second, 
	                    start_alt-4, 0);
					}
      			} 
				else{
					 state_flag = 5;
					 //ROS_INFO("state_flag = 5");
					 sleep(3);
				} 

		}
		if (state_flag==5)//到达最后一个航点
		{
            //save_waypoints2_to_csv("/home/jetson/yahboomcar_ws/src/iq_gnc/src/waypoints2.csv", waypoints2);//存点
            ROS_INFO("return to home");
            set_destination_home(0,0,2);
            sleep(1);
            // 判断是否接近目标（0,0,3）
            double dx = t_wb[0];
            double dy = t_wb[1];
            //double dz = t_wb[2] - 5;

            double distance = sqrt(dx*dx + dy*dy);
            if (distance < 1) {
                ROS_INFO("Reached return point, initiating landing.");
                land();
                break;
            }
		}	 
		
	}
	return 0;
}
